///////////////////////////////////////////////////////////
// 
//  MyWindow.cpp
//  
//  Authors:  Jeremy Bennett
//
///////////////////////////////////////////////////////////

// Local Includes

#include "stdafx.h"          // Required PCH
#include "MyWindow.h"        //
#include "MyBox.h"           //
#include "MySphere.h"        //
#include "MyCylinder.h"      //
#include "MySquare.h"        //
#include "MyRectangle.h"     //
#include "MyTriangle.h"      //
#include "MyStick.h"         //
#include "MyOozeBall.h"      //
#include "GL_ShapeDrawer.h"  // TOXIC: Refactor
#include <iostream>
#include <fstream>



#if defined(USE_MIDI_FUNCTIONS)
#include "PlayMidiLib.h"
#endif

// SDL Includes
#include <sdl.h>
#include <SDL_opengl.h>

// SDL Mixer Includes
//#include "SDL_mixer.h"

// Bullet Includes
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h>
#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
#include <BulletCollision/BroadphaseCollision/btAxisSweep3.h>
#include <BulletCollision/CollisionShapes/btCollisionShape.h>
#include <BulletCollision/CollisionShapes/btSphereShape.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>

#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
#include <BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h>
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
#include <BulletDynamics/Dynamics/btDynamicsWorld.h>

#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
#include <BulletSoftBody/btSoftSoftCollisionAlgorithm.h>
#include <BulletSoftBody/btSoftRigidCollisionAlgorithm.h>
#include <BulletSoftBody/btSoftRigidDynamicsWorld.h>
#include <BulletSoftBody/btSoftBodyHelpers.h>

#include <LinearMath/btDefaultMotionState.h>
#include <LinearMath/btQuaternion.h>
#include <LinearMath/btMatrix3x3.h>
#include <LinearMath/btVector3.h>

// System Includes
#include <time.h>
#include <stdio.h>
#include <iostream>

template <class T> inline T min (const T &x, const T &y) { return (x < y) ? x : y; }
template <class T> inline T max (const T &x, const T &y) { return (x > y) ? x : y; }
template <class T> inline T clamp (const T &x, const T &l, const T &r ) { return min(max(x,l),r); }

const char* pFile[] = { "Unknown.txt", 
                        "Box.txt",
                        "Sphere.txt",
                        "Cylinder.txt",
                        "Square.txt",
                        "Triangle.txt",
                        "Rectangle.txt",
                        "Stick.txt",
                        "Ooze.txt" };

/*
Mix_Chunk *pSnd1 = NULL;
Mix_Music *pMsc1 = NULL;
*/
int iChanSnd1 = -1;

void musicDone()
{
    /*Mix_HaltMusic();*/
    //Mix_FreeMusic(music);
    //music = NULL;
}


//                                         | Mote 1    | Mote 2     |
const unsigned int s_uiMidiInstruments[] = { 48, 35, 42, 45, 38, 49 };

/*
//by default, Bullet will use its own nearcallback, but you can override it using dispatcher->setNearCallback()
void customNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
{
		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;

		if (dispatcher.needsCollision(colObj0,colObj1))
		{
			//dispatcher will keep algorithms persistent in the collision pair
			if (!collisionPair.m_algorithm)
			{
				collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
			}

			if (collisionPair.m_algorithm)
			{
				btManifoldResult contactPointResult(colObj0,colObj1);

				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
				{
					//discrete collision detection query
					collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
				} else
				{
					//continuous collision detection query, time of impact (toi)
					float toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
					if (dispatchInfo.m_timeOfImpact > toi)
						dispatchInfo.m_timeOfImpact = toi;

				}
			}
		}

}
*/

void MyIntTickCB(btDynamicsWorld *world, btScalar timeStep)
{
#if 0
    int nMan = world->getDispatcher()->getNumManifolds();
    for ( int i=0 ; i<nMan ; i++ ) {
		btPersistentManifold* pMan = world->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject* pColObjA = static_cast<btCollisionObject*>(pMan->getBody0());
		btCollisionObject* pColObjB = static_cast<btCollisionObject*>(pMan->getBody1());

        btSoftBody *pSftBdyA = btSoftBody::upcast(pColObjA);
        btSoftBody *pSftBdyB = btSoftBody::upcast(pColObjB);

		int nPnt = pMan->getNumContacts();
		for (int j=0 ; j<nPnt ; j++ ){
			btManifoldPoint& rPnt = pMan->getContactPoint(j);

            int   iLife   = rPnt.getLifeTime();
            float fImpule = rPnt.getAppliedImpulse();

            if ( iLife < 2 && fImpule > 1.0f ) {
                /*Mix_PlayChannel(-1, pSnd1, 1)*/;
            }

            /*
			if ( pt.getDistance() < 0.f ) {
				const btVector3& ptA = pt.getPositionWorldOnA();
				const btVector3& ptB = pt.getPositionWorldOnB();
				const btVector3& normalOnB = pt.m_normalWorldOnB;
			}
            */
		}
	}
#endif
}

MyWindow::MyWindow()
: // Splash
  // Games 
  _iTotalHappiness(0),
  // Test
  _x(640),
  _y(400),
  _lx(640),
  _ly(400),
  _rx(640),
  _ry(400),
  // Bullet World
  _aBltColShapes(),
  _pBltDynWorld(NULL),
  _pBltBroadphase(NULL),   
  _pBltDispatcher(NULL),   
  _pBltConstrSolver(NULL), 
  _pBBoxCrtFunc(NULL),     
  _pDefColConf(NULL),
  //Simulation
  _pStickShape(NULL),
  _pActiveShape(NULL),
  _bShapeAtMarker(false),
  _dtotalMoves(0),
  _iConfigNum(0),
  // Bullet Picking/Manipulation
  _pPickConstr(NULL),
  _pPickRigBdy(NULL),
  _vPickREnd( 0.0f, 0.0f, 0.0f ),   
  _vPickPos( 0.0f, 0.0f, 0.0f ),    
  _fPickDist(0.0f),
  _pPickedShape(NULL),
  // Bullet Rendering
  _pBltGLDrawer(NULL),
  _bltClock(),
  _bBabble(false),
  _bEval(false),
  _fStickDis(0.0f),
  _nStickMovIter(0),
  _vStickMov(0.0f,0.0f,0.0f),
  _vStickPos(0.0f,0.0f,0.0f),
  _vBabOrigPos(0.0f,0.0f,0.0f),
  _iBabSinceReset(0),
  _iBabOrient(0),
  _eBabPhase(Babble_Init),
  _eEvalPhase(Eval_Init),
  _atMaxPushes(false),
  _maxPushes(500),
  _vMarkerPosition(12.0f,0.0f,-12.5f),
  _fMarkerAngle(0.0f),
  // Common
  _simulation(),
  _context(),
  _pSurface(NULL),
  _xCont(0),
  // Window
  _w(1280),
  _h(700),
  _vCamPos(0,0,0),
  _vCamTargPos(0,0,17),
  _vCamUp(0,1,0),
  _fCamDis(50.0f),
  _fEle(20.0f),
  _fAzi(0.0f),
  _iForwardAxis(2),
  // State
  _bPick(true),
  _bSplash(false),
  _bQuit(false)
{
 #if defined(USE_MIDI_FUNCTIONS)
	if(StartMidiOutForiDev(0)==MIDI_OUT_ERROR){
	   ("Couldn't open Midi Out - perhaps it is already in use for another application.\n\n"
		"Press return to exit."
	   );
	  getchar();
	  exit(0);
	 }
#endif

    _vDbgPos[0] = btVector3(0.0f,0.0f,0.0f);
    _vDbgPos[1] = btVector3(0.0f,0.0f,0.0f);
    _fDbgAng[0] = 0.0f;
    _fDbgAng[1] = 0.0f;

    // Initialize random number generator
    srand(unsigned int(time(0)));  
    
    _pMarker = new Marker();
    _initPhysics();
}

MyWindow::~MyWindow()
{
  #if defined(USE_MIDI_FUNCTIONS)
	StopMidiOut();
  #endif

    if (_pMarker) delete _pMarker;
}

bool
MyWindow::init()
{
    // Initialize SDL (Note: video is required to start event loop)
	if ( SDL_Init(SDL_INIT_VIDEO|SDL_INIT_AUDIO) < 0 ) {
		printf("ERROR - Initialization Failed: %s\n",SDL_GetError());
		return false;
	}

    // Initialize GL
    if (!_initGL())       return false;
    if (!_initAudio())    return false;

    // Configure the context
    if (!_context.configure()) return false;
    
    /*_simulation.test();
    _simulation.test2();*/
	//_simulation.deserialize("foo.txt");

    return true;
}

bool
MyWindow::fini()
{
    _finiPhysics();
    _finiAudio();

    SDL_Quit();

    return true;
}


bool 
MyWindow::setViewPort( int x, int y, int w, int h )
{
    glViewport( x, y, w, h );
    glMatrixMode( GL_PROJECTION );
	glLoadIdentity( );

	glOrtho( x, w, y, h, -200.0, 400.0 );

	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity( );

    return true;
}

bool 
MyWindow::execute( int argv, char* argc[] )
{
    _bQuit = false;
    
    while  ( !_bQuit ) {
            if (!_preFrame())  break;
            if (!_render())    break;
            if (!_postFrame()) break;
    }

    return true;
}

bool 
MyWindow::_initGL()
{
    printf("***** Initializing OpenGL Window ***** \n");

    int bpp = SDL_GetVideoInfo()->vfmt->BitsPerPixel;
    unsigned int myFlags = SDL_OPENGL;
                       //| SDL_FULLSCREEN;


    SDL_GL_SetAttribute( SDL_GL_DOUBLEBUFFER, 1 );
    SDL_GL_SetAttribute( SDL_GL_ACCELERATED_VISUAL, 1 );
    SDL_GL_SetAttribute( SDL_GL_SWAP_CONTROL, 0 );

    _pSurface = SDL_SetVideoMode( _w, _h, bpp, myFlags );
    if ( _pSurface == NULL ) {
		printf("ERROR - SetVideoMode Failed: %s\n",SDL_GetError());
		return false;
	} 
    
	GLfloat light_ambient[]  = { 0.2f, 0.2f, 0.2f, 1.0f };
	GLfloat light_diffuse[]  = { 1.0f, 1.0f, 1.0f, 1.0f };
	GLfloat light_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
	/*	light_position is NOT default value	*/
	GLfloat light_position0[] = {  10.0f, 0.0f, 60.0f, 0.0f };
	GLfloat light_position1[] = { -10.0f, 0.0f, 60.0f, 0.0f };

	//glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
	//glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);

	//lLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
	//glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
	glLightfv(GL_LIGHT1, GL_POSITION, light_position1);

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);

	glShadeModel(GL_SMOOTH);

    setViewPort(0,0,_w,_h);

    //glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
    //glBlendFunc(GL_SRC_ALPHA,GL_DST_ALPHA);
    //glEnable(GL_BLEND);

    printf("***** Initializing OpenGL Complete ***** \n");
    
#if 0
    printf("\n");
	printf( "Vendor     : %s\n", glGetString( GL_VENDOR ) );
	printf( "Renderer   : %s\n", glGetString( GL_RENDERER ) );
	printf( "Version    : %s\n", glGetString( GL_VERSION ) );
	printf( "Extensions : %s\n", glGetString( GL_EXTENSIONS ) );
	printf("\n");
#endif

    SDL_WM_SetCaption( "Shafttu -- Shapes Affordances Through Tool Use", "Honk, Honk" );

    return true;
}

bool 
MyWindow::_initAudio()
{
    /*
    //
    int audio_rate = 22050;
    Uint16 audio_format = AUDIO_S16;
    int audio_channels = 2;
    int audio_buffers = 4096;

    if(Mix_OpenAudio(audio_rate, audio_format, audio_channels, audio_buffers)) {
        printf("Unable to open audio!\n");
        exit(1);
    }

    Mix_QuerySpec(&audio_rate, &audio_format, &audio_channels);

    pSnd1 = Mix_LoadWAV("Snd1.wav");
    pMsc1 = Mix_LoadMUS("Msc1.wav");

    Mix_PlayMusic(pMsc1, -1);
    */

    return true;
}

bool 
MyWindow::_stopAudio()
{
    if ( iChanSnd1 != -1 ) {
        /*Mix_HaltChannel(iChanSnd1);*/ 
        iChanSnd1 = -1;
    }

    return true;
}

bool 
MyWindow::_finiAudio()
{
    /*
    Mix_HaltMusic();
    
    Mix_FreeChunk(pSnd1);
    Mix_FreeMusic(pMsc1);

    //
    Mix_CloseAudio();
    */

    return true;
}

bool 
MyWindow::_updateTime()
{
    float tMS = (btScalar)_bltClock.getTimeMicroseconds();
    _bltClock.reset();

    // Increment the dynamic world time
    //_pBltDynWorld->stepSimulation( tMS / 1000000.f);
    _pBltDynWorld->stepSimulation( tMS / 100.f);

    // Perform garbage collection onf the soft body world
    _infoSofBdyWorld.m_sparsesdf.GarbageCollect();

    return true;
}

bool 
MyWindow::_updateCamera()
{
    glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	
    float rele = _fEle * 0.01745329251994329547f; // rads per deg
	float razi = _fAzi * 0.01745329251994329547f; // rads per deg

	btQuaternion rot(_vCamUp,razi);

	btVector3 eyePos(0,0,0);
	eyePos[_iForwardAxis] = -_fCamDis;

	btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
	if (forward.length2() < SIMD_EPSILON) {
		forward.setValue(1.f,0.f,0.f);
	}
	btVector3 right = _vCamUp.cross(forward);
	btQuaternion roll(right,-rele);

	eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;

	_vCamPos[0] = eyePos.getX();
	_vCamPos[1] = eyePos.getY();
	_vCamPos[2] = eyePos.getZ();
	_vCamPos += _vCamTargPos;

	if ( _w == 0 && _h == 0 )
		return true;

	btScalar aspect;
	btVector3 extents;

	if ( _w > _h ) {
		aspect = _w / (btScalar)_h;
		extents.setValue(aspect * 1.0f, 1.0f,0);
	} else {
		aspect = _h / (btScalar)_w;
		extents.setValue(1.0f, aspect*1.f,0);
	}
	
	// reset matrix
	glLoadIdentity();
			
	extents *= _fCamDis;
	btVector3 lower = _vCamTargPos - extents;
	btVector3 upper = _vCamTargPos + extents;
	glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

    return true;
}

bool 
MyWindow::_initPhysics()
{
    // Initialize the shape drawer
    _pBltGLDrawer = new GL_ShapeDrawer();
    //_pBltGLDrawer->enableTexture(true);

    // Initialize the collision configuration
    _pDefColConf = new btDefaultCollisionConfiguration();

    // Initialize the collision dispatcher
    _pBltDispatcher = new	btCollisionDispatcher(_pDefColConf);
    _infoSofBdyWorld.m_dispatcher = _pBltDispatcher;
    
    // Initialize the broad phase
    const int maxProxies = 32766;
    btVector3 worldAabbMin(-180,-180,-30);
	btVector3 worldAabbMax(180,180,30);
	_pBltBroadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
    _infoSofBdyWorld.m_broadphase = _pBltBroadphase;

    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
    _pBltConstrSolver = solver;

    // Initialize dynamic world
    _pBltDynWorld = new btDiscreteDynamicsWorld(_pBltDispatcher,_pBltBroadphase,_pBltConstrSolver,_pDefColConf);
    _pBltDynWorld->getDispatchInfo().m_enableSPU = true; 
    _pBltDynWorld->setGravity(btVector3(0,0,10));
    _pBltDynWorld->setInternalTickCallback(MyIntTickCB,NULL);

    // Initalize the softbody
    _infoSofBdyWorld.m_gravity.setValue(0,0,-10);
    _infoSofBdyWorld.m_sparsesdf.Initialize();

    // Create walls
    _createStaticBltBox( btVector3(180.0f,10.0f,1000.0f), btVector3(0,-60,0) ); // Bottom
    _createStaticBltBox( btVector3(180.0f,10.0f,1000.0f), btVector3(0,60,0)  ); // Top
    _createStaticBltBox( btVector3(10.0f,180.0f,1000.0f), btVector3(90,0,0)  ); // Right
    _createStaticBltBox( btVector3(10.0f,180.0f,1000.0f), btVector3(-90,0,0) ); // Left
    MyBox *pBox = (MyBox*)_createStaticBltBox( btVector3(200.0f,140.0f,10.0f), btVector3(0,0,0)); // Back
    pBox->setColor(btVector4(0,0,0,1));


	//create table
	_pTable = (MyShape*)_createDynamicBltRectangle( btVector3(75.0f,48.0f,0.5f), btVector3(0.0f, 0.0f,-10.0f), 0.0);
	_pTable->setColor(btVector4(2.15f,2.15f,2.15f,1.0f));

	//_pTable = (MyShape*)_createDynamicBltRectangle( btVector3(25.0f,25.0f,0.5f), btVector3(0,0,-12.0f), 0.0f);
    //if ( !_initConfig() ) {
    //    // Create dynamics cubes
    //    for ( int x=0 ; x<10 ; x++ ) {
    //        for ( int y=0 ; y<10 ; y++ ) {
    //            _createDynamicBltSphere( 2.0f, btVector3(-20.0f+x*4.0f,-20.0f+y*4.0f,-20.0f) );
    //            //_createDynamicOozeBall( 2.0f, btVector3(-20.0f+x*4.0f,-20.0f+y*4.0f,-20.0f) );
    //        }
    //    }
    //}

	//create shapes
	
    // TOXIC: Use number keys to create shapes
	//_pActiveShape = (MyShape*) _createDynamicBltSquare( btVector3(6.0f,6.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f));
	//_pActiveShape = (MyShape*) _createDynamicBltTriangle(12.0f, 4.0f, btVector3(0.0f, 0.0f,-20.0f));
	//_pActiveShape = (MyShape*) _createDynamicBltRectangle( btVector3(12.0f,6.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f));
	//_pActiveShape = (MyShape*) _createDynamicBltCylinder( btVector3(6.0f,7.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f) );

	 _pStickShape = (MyShape*) _createDynamicBltStick( btVector3(0.5f,0.5f,2.0f), btVector3(0.0f, -20.0f,-20.0f), 5.0f);
	 
     
     /*_pActiveShape->setColor(btVector4(1.0f,0.0f,1.0f,1.0f));
	 _pStickShape->setColor(btVector4(0.0f,0.0f,0.0f,1.0f));*/

#if 0
	 if ( _pStickConstr == NULL ) {
			_pStickShape->setActivationState(DISABLE_DEACTIVATION);
      
			btScalar	          m[16];
			btDefaultMotionState *pMotState = (btDefaultMotionState*)_pStickShape->getMotionState();
			pMotState->m_graphicsWorldTrans.getOpenGLMatrix(m);

			btVector3 vPos(m[12],m[13],m[14]);
			btVector3 vLocPiv = _pStickShape->getCenterOfMassTransform().inverse() * vPos;

			// Create and add a constraint to the world
			btPoint2PointConstraint* pP2P = new btPoint2PointConstraint(*_pStickShape,vLocPiv);
			pP2P->m_setting.m_impulseClamp = 30.0f;
			pP2P->m_setting.m_tau = 0.1f;
			_pBltDynWorld->addConstraint(pP2P);

			// Track the picking event
			_pStickConstr = pP2P;
		}
#endif


	 /*_createDynamicBltSquare( btVector3(6.0f,6.0f,2.0f), btVector3(15.0f, 0.0f,-20.0f));
	 _createDynamicBltTriangle(12.0f, 4.0f, btVector3(15.0f, 0.0f,-20.0f));
	 _createDynamicBltRectangle( btVector3(12.0f,6.0f,2.0f), btVector3(15.0f, 0.0f,-20.0f));
	 _createDynamicBltCylinder( btVector3(6.0f,7.0f,2.0f), btVector3(15.0f, 0.0f,-20.0f) );*/

	
    int nColObj = _pBltDynWorld->getNumCollisionObjects();

    // Obtain a copy of the collision object array
    btCollisionObjectArray aColObj = _pBltDynWorld->getCollisionObjectArray(); // TOXIC: Why copy?

    for ( int i=0 ; i<nColObj ; i++ ) {
        btCollisionObject* pColObj = aColObj[i];
		
        btRigidBody*       pRigBdy = btRigidBody::upcast(pColObj);
        if ( pRigBdy ) {
            // Obtain the motion state if present
            if (pRigBdy->getMotionState()) {
				    btDefaultMotionState* pMotState = (btDefaultMotionState*)pRigBdy->getMotionState();
				    pMotState->m_graphicsWorldTrans = pMotState->m_startWorldTrans;
				    pRigBdy->setCenterOfMassTransform( pMotState->m_graphicsWorldTrans );
				    pRigBdy->setInterpolationWorldTransform( pMotState->m_startWorldTrans );
				    pRigBdy->forceActivationState(ACTIVE_TAG);
				    //pRigBdy->activate();
				    pRigBdy->setDeactivationTime(0);
		    }

            // If not a static obj set linear/angular velocity
            if ( !pRigBdy->isStaticObject() ) {
                pRigBdy->setLinearVelocity(btVector3(0,0,0));
                pRigBdy->setAngularVelocity(btVector3(0,0,0));
            }
        }
    }

    _infoSofBdyWorld.m_sparsesdf.Reset();
    _infoSofBdyWorld.air_density		=	(btScalar)1.2;
	_infoSofBdyWorld.water_density	=	0;
	_infoSofBdyWorld.water_offset		=	0;
	_infoSofBdyWorld.water_normal		=	btVector3(0,0,0);

    /*
    for ( int i=0 ; i<nColObj ; i++ ) {
        btCollisionObject *pColObj = aColObj[i];
        btRigidBody       *pRigBdy = btRigidBody::upcast(pColObj);
        MyShape           *pShp    = dynamic_cast<MyShape*>(pRigBdy);

        if ( pShp && pShp->type() == my_ooze ) {

            MyOozeBall *pOozeBall = (MyOozeBall*)pShp;

            pRigBdy->setActivationState(DISABLE_DEACTIVATION);
            pOozeBall->setIsPicked(true);

            btVector3 vLocPiv = pRigBdy->getCenterOfMassPosition();

            // Create and add a constraint to the world
            btPoint2PointConstraint* pP2P = new btPoint2PointConstraint(*pRigBdy,vLocPiv);
            pP2P->m_setting.m_impulseClamp = 30.0f;
            pP2P->m_setting.m_tau = 0.1f;
            _pBltDynWorld->addConstraint(pP2P);

            _updateTime();

            // Remove and delete the pick constraint
            _pBltDynWorld->removeConstraint(pP2P);
            delete pP2P;

            // Place the picked body back into a activation state
            pRigBdy->forceActivationState(ACTIVE_TAG);
            pRigBdy->setDeactivationTime(0.0f);
            pOozeBall->setIsPicked(false);
        }
    }
    */

    //btRigidBody* pTest->applyCentralImpulse( btVector3( 20.0f, 0.0f, 0.0f ) ); // Cause initial movement!

	/*
	btPoint2PointConstraint* pP2P = static_cast<btPoint2PointConstraint*>(_pStickConstr);
	if (!pP2P){
							
		btTransform newTrans;
		_pStickShape->getMotionState()->getWorldTransform(newTrans);
		newTrans.getOrigin() = btVector3(15.0f, 0.0f,-20.0f);
		_pStickShape->getMotionState()->setWorldTransform(newTrans);

	}else{
		btVector3 newTrans =  btVector3(15.0f, 0.0f,-20.0f);
		btVector3 oPivot = pP2P->getPivotInB(); 
		oPivot.setX(newTrans.x());
		oPivot.setY(newTrans.y());
		oPivot.setZ(newTrans.z());
	}
	*/

    return true;
}

bool 
MyWindow::_finiPhysics()
{   
    // Delete collision objects on the dynamic world
    int nColObj = _pBltDynWorld->getNumCollisionObjects();
    for ( int i=nColObj-1 ; i>=0 ; i-- ) {
        // Obtain the collision object
        btCollisionObject *pColObj = _pBltDynWorld->getCollisionObjectArray()[i];
		
        // If a rigid body and it has a motion state delete it
        btRigidBody       *pRigBdy = btRigidBody::upcast(pColObj);
        if ( pRigBdy && pRigBdy->getMotionState() ) {
            btMotionState *pMotState = pRigBdy->getMotionState();
            delete pMotState;
        }

        // Remove the collision object and delete
        _pBltDynWorld->removeCollisionObject(pColObj);
        if (pColObj) delete pColObj;
    }

    // Delete collision shapes
    int nColShp = _aBltColShapes.size();
    for ( int i=0 ; i< nColShp; i++ ) {
		btCollisionShape *pShp = _aBltColShapes[i];
		if (pShp) delete pShp;
	}
    _aBltColShapes.resize(0);

    // Delete allocated objects
	if (_pBltDynWorld)     delete _pBltDynWorld;      _pBltDynWorld     = NULL;
    if (_pBltBroadphase)   delete _pBltBroadphase;    _pBltBroadphase   = NULL;  
	if (_pBltDispatcher)   delete _pBltDispatcher;    _pBltDispatcher   = NULL;  
	if (_pBltConstrSolver) delete _pBltConstrSolver;  _pBltConstrSolver = NULL;
	if (_pBBoxCrtFunc)     delete _pBBoxCrtFunc;      _pBBoxCrtFunc     = NULL;    
	if (_pDefColConf)      delete _pDefColConf;       _pDefColConf      = NULL;     
    
    return true;
}

bool 
MyWindow::_resetBulletScene()
{   
	
    _finiPhysics();
	_initPhysics();

    return true;
}

bool 
MyWindow::_renderBulletScene()
{
    // Local variables
    btScalar	m[16];
	btMatrix3x3	rot; rot.setIdentity();
	btVector3   wireColor(1,0,0);
	
    const int nColObj = _pBltDynWorld->getNumCollisionObjects();
    for( int i=0 ; i<nColObj ; i++ ) {
        btCollisionObject*	pColObj = _pBltDynWorld->getCollisionObjectArray()[i];
		btRigidBody*		pRigBdy = btRigidBody::upcast(pColObj);
        btSoftBody*		    pSofBdy = btSoftBody::upcast(pColObj);
        MyShape*            pShp    = NULL;

        if ( pSofBdy ) {
            for( int i=0 ; i<pSofBdy->m_links.size() ; ++i )
	        {
		        const btSoftBody::Link&	l=pSofBdy->m_links[i];
                btVector3 lCol( 0.0f, 1.0f, 0.0f );
		        _drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lCol);
	        }
        } else {
            try {
                pShp    = dynamic_cast<MyShape*>(pColObj);
            } catch(...) {
            }

            if ( pShp ) {
                pShp->render( &_context );
            } else {

                if ( pRigBdy && pRigBdy->getMotionState() ) {
                    btDefaultMotionState *pMotState = (btDefaultMotionState*)pRigBdy->getMotionState();
                    pMotState->m_graphicsWorldTrans.getOpenGLMatrix(m);
                    rot = pMotState->m_graphicsWorldTrans.getBasis();
                } else {
                    pColObj->getWorldTransform().getOpenGLMatrix(m);
                    rot = pColObj->getWorldTransform().getBasis();
                }

                // Obtain the min/max bbox
                btVector3 aabbMin,aabbMax;
		        _pBltDynWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
                aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
		        aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);

                _pBltGLDrawer->drawOpenGL(m,pColObj->getCollisionShape(),wireColor,0,aabbMin,aabbMax);

            }
        }
    }

    if ( _bPick ) {
        _iTotalHappiness = 0;

        //const int nColObj = _pBltDynWorld->getNumCollisionObjects();
        for( int i=0 ; i<nColObj ; i++ ) {
            btCollisionObject*	pColObj = _pBltDynWorld->getCollisionObjectArray()[i];
		    btRigidBody*		pRigBdy = btRigidBody::upcast(pColObj);

            MyShape* pShp = NULL;

            try {
                pShp = dynamic_cast<MyShape*>(pColObj);
            } catch(...) {
            }

            if ( pShp && (pShp->type() == my_ooze) ) {
                ((MyOozeBall*)pShp)->findNeighbors(_pBltDynWorld);
                _iTotalHappiness += ((MyOozeBall*)pShp)->happiness();
            }
        }
    }

    return true;
}

bool	
MyWindow::_drawLine( const btVector3& from, const btVector3& to, const btVector3& color )
{
		glBegin(GL_LINES);
		glColor4f(color.getX(), color.getY(), color.getZ(),1.f);
		glVertex3d(from.getX(), from.getY(), from.getZ());
		glVertex3d(to.getX(), to.getY(), to.getZ());
		glEnd();

        return true;
}

bool 
MyWindow::_renderPointer()
{
    static const float s_ptr_width = 20.0f;

    const XINPUT_STATE& xState = _xCont.xState();

    printf("Left(%hd,%hd) Right(%hd,%hd)\n", xState.Gamepad.sThumbLX, 
                                             xState.Gamepad.sThumbLY, 
                                             xState.Gamepad.sThumbRX, 
                                             xState.Gamepad.sThumbRY );
    
    int lx = xState.Gamepad.sThumbLX/4600,
        ly = xState.Gamepad.sThumbLY/4600,
        rx = xState.Gamepad.sThumbRX/4600,
        ry = xState.Gamepad.sThumbRY/4600;

    _lx += lx;
    if ( _lx < 0 ) _lx = 0.0f; if ( _lx > _w ) _lx = float(_w);

    _ly += ly;
    if ( _ly < 0 ) _ly = 0.0f; if ( _ly > _w ) _ly = float(_h);

    _rx += rx;
    if ( _rx < 0 ) _rx = 0.0f; if ( _rx > _w ) _rx = float(_w);

    _ry += ry;
    if ( _ry < 0 ) _ry = 0.0f; if ( _ry > _w ) _ry = float(_h);

    const float fCol[4] = { 1.0f, 0.0f, 0.0f, 1.0f };

    /*
    _context.renderTexRect( PLAYER_POINTER_TEX, fCol,
                            _lx-s_ptr_width, 
                            _ly-s_ptr_width, 
                            2*s_ptr_width, 2*s_ptr_width );

    _context.renderTexRect( PLAYER_POINTER_TEX, fCol,
                            _rx-s_ptr_width, 
                            _ry-s_ptr_width, 
                            2*s_ptr_width, 2*s_ptr_width );
    */

    return true;
}

btVector3 
MyWindow::_createPickRay( int x, int y )
{
    btVector3 rEnd;

    btScalar aspect;
	btVector3 extents;
	if ( _w > _h ) {
		aspect = _w / (btScalar)_h;
		extents.setValue(aspect * 1.0f, 1.0f,0);
	} else {
		aspect = _h / (btScalar)_w;
		extents.setValue(1.0f, aspect*1.f,0);
	}
	
	extents *= _fCamDis;
	btVector3 lower = _vCamTargPos - extents;
	btVector3 upper = _vCamTargPos + extents;

	btScalar u = x / btScalar(_w);
	btScalar v = (_h-y) / btScalar(_h);
	
	rEnd.setValue( (1.0f-u)*lower.getX()+u*upper.getX(),
                   (1.0f -v)*lower.getY()+v*upper.getY(),
                   _vCamTargPos.getZ() );

    return rEnd;
}

btRigidBody*
MyWindow::_createStaticBltBox( const btVector3 &vSize, 
                               const btVector3 &vOrigin )
{
    btRigidBody* pRigbdy = new MyBox(vSize,vOrigin,0.0f);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltBox( const btVector3 &vSize, 
                                const btVector3 &vOrigin, 
                                const btScalar   mass )
{
    btRigidBody* pRigbdy = new MyBox(vSize,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltSphere( const btScalar   fRadius, 
                                   const btVector3 &vOrigin, 
                                   const btScalar   mass )
{
    btRigidBody* pRigbdy = new MySphere(fRadius,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltCylinder( const btVector3 &vSize, 
                                     const btVector3 &vOrigin, 
                                     const btScalar   mass )
{
    btRigidBody* pRigbdy = new MyCylinder(vSize,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltSquare( const btVector3 &vSize, 
                                   const btVector3 &vOrigin, 
                                   const btScalar   mass )
{
    btRigidBody* pRigbdy = new MySquare(vSize,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltRectangle( const btVector3 &vSize, 
                                      const btVector3 &vOrigin, 
                                      const btScalar   mass )
{
    btRigidBody* pRigbdy = new MyRectangle(vSize,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltTriangle( const btScalar vScale, 
                                     const btScalar vThickness,
                                     const btVector3 &vOrigin, 
                                     const btScalar   mass )
{
    btRigidBody* pRigbdy = new MyTriangle(vScale, vThickness, vOrigin, mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicBltStick( const btVector3 &vSize, 
                                     const btVector3 &vOrigin, 
                                     const btScalar   mass )
{
    btRigidBody* pRigbdy = new MyStick(vSize,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

btRigidBody* 
MyWindow::_createDynamicOozeBall( const btScalar   fRadius, 
                                  const btVector3 &vOrigin, 
                                  const btScalar   mass)
{
    btRigidBody* pRigbdy = new MyOozeBall(fRadius,vOrigin,mass);
    _pBltDynWorld->addRigidBody(pRigbdy);
    return pRigbdy;
}

bool
MyWindow::_preFrame()
{
    bool bRes = true;

    // Otherwise, let the window handle the event
    bRes &= _handleEvents();

    if ( _bBabble ) {
        _runBabble();
    }

	 if ( _bEval ) {
        _runEval();
    }

	if ( _nStickMovIter != 0 ) {

        btVector3 vStick = _pStickShape->getWorldTransform().getOrigin() - _vStickPos;
        float fDis = vStick.length();

        if ( fDis < _fStickDis ) {
            _pStickShape->setLinearVelocity(_vStickMov);
            _nStickMovIter--;
        } else {
            // Stop the stick
		    _nStickMovIter = 0;
            _fStickDis     = 0.f;
            _vStickMov     = btVector3(0.0f,0.0f,0.0f);
            _pStickShape->setLinearVelocity(_vStickMov);
        }

	} else {

        if (_pActiveShape) {
            _vDbgPos[1] = _pActiveShape->getWorldTransform().getOrigin();
		    _fDbgAng[1] = 180.0f/PI*_pActiveShape->getWorldTransform().getRotation().getAngle();
            _qB = _pActiveShape->getWorldTransform().getRotation();
        }

        _nStickMovIter = 0;
        _fStickDis     = 0.f;
        _vStickMov     = _pStickShape->getLinearVelocity();
        _vStickMov[0]  = 0.0f;
        _vStickMov[1]  = 0.0f;
        _pStickShape->setLinearVelocity(_vStickMov);
    }

    return true;
}

bool
MyWindow::_postFrame()
{
    bool bRes = true;

    _stopAudio();

    return bRes;
}

bool
MyWindow::_render()
{
	//std::cout<<"\n\n\nstick x-position: "<<_pStickShape->getWorldTransform().getOrigin().x()<<std::endl;
	/*std::cout<<"activeshape z-position: "<<_pActiveShape->getWorldTransform().getOrigin().z()<<std::endl;
	std::cout<<"table z-position: "<<_pTable->getWorldTransform().getOrigin().z()<<std::endl;*/

    glClearColor( 0.0, 0.0, 0.0, 1.0 );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    if (_bSplash) {

        const float fCol[4] = { 1.0f, 1.0f, 1.0f, 1.0f };
        _context.renderTexRect( SPLASH_SCREEN, fCol, 0.0f, 0.0f, float(_w), float(_h) );

    } else {

        _updateTime();
        _updateCamera();

        float aCol[4] = { 1.0f, 1.0f, 1.0f, 1.0f } ;
        _context.renderTexRect( WINDOW_BACKGROUND_TEX, aCol, -80, -50, 160, 100 );
       // _context.renderTexRect( MENU_TEX, aCol, 50, -50, 30, 100 );

        _renderBulletScene();

        if (this->_bBabble) {
            glLineWidth(1.0f);
		    glColor4fv( _vCol );
		    glBegin(GL_LINES);
			    glVertex3f(_rStart.x(), _rStart.y(), _rStart.z());
			    glVertex3f(_rEnd.x(), _rEnd.y(), _rEnd.z());
		    glEnd();
		    glColor4fv( btVector4(0.0f,0.0f,1.0f,1.0f) );
		    glBegin(GL_LINES);
			    glVertex3f(_rEnd.x(), _rEnd.y(), _rEnd.z());
			    glVertex3f(_rCenter.x(), _rCenter.y(), _rCenter.z());
		    glEnd();
		    glColor4fv( btVector4(1.0f,1.0f,1.0f,1.0f) );
		    glBegin(GL_LINES);
			    glVertex3f(_rHit.x(), _rHit.y(), _rHit.z());
			    glVertex3f(_rNorm.x(), _rNorm.y(), _rNorm.z());
		    glEnd();
        }

        // Render the marker if present and currently has shape
        if (_pMarker && _pMarker->hasShape() ) 
            _pMarker->render();

        if ( _bEval ) {

            glLineWidth(1.0f);
            glDisable(GL_LIGHTING);

            unsigned int nPnts = 0;

            // Render the predicted points
            nPnts = _vEvalPred.size();
            glColor4fv( btVector4(1.0f,1.0f,0.0f,1.0f) );
            glBegin(GL_LINES);
            for ( unsigned int n=0 ; n<nPnts ; n+=2 ) {   
			        glVertex3f(_vEvalPred[n+0].e[0],_vEvalPred[n+0].e[1],_vEvalPred[n+0].e[2]);
                    glVertex3f(_vEvalPred[n+1].e[0],_vEvalPred[n+1].e[1],_vEvalPred[n+1].e[2]);
            }
		    glEnd();

            // Render the actual points
            nPnts = _vEvalAct.size();
            glColor4fv( btVector4(1.0f,1.0f,1.0f,1.0f) );
            glBegin(GL_LINES);
            for ( unsigned int n=0 ; n<nPnts ; n+=2 ) {   
			        glVertex3f(_vEvalAct[n+0].e[0],_vEvalAct[n+0].e[1],_vEvalAct[n+0].e[2]);
                    glVertex3f(_vEvalAct[n+1].e[0],_vEvalAct[n+1].e[1],_vEvalAct[n+1].e[2]);
            }
		    glEnd();
            glEnable(GL_LIGHTING);
        }

	}

#if 1
    //char aChar[100];
    //sprintf( aChar, "Total Happiness: %d", _iTotalHappiness );
    //// 1280x800
    //_context.renderText( aChar, 1060, 745 );

    //MyOozeBall *pSpeciman = (MyOozeBall*)_pPickedShape;
    //if ( _pPickedShape && _pPickedShape->type() == my_ooze ) {
    //    sprintf( aChar, "Specimen: %d", pSpeciman->id() );
    //    _context.renderText( aChar, 1060, 370 );
    //    sprintf( aChar, "Factor 0: %d", pSpeciman->factor(0) );
    //    _context.renderText( aChar, 1060, 330 );
    //    sprintf( aChar, "Factor 1: %d", pSpeciman->factor(1) );
    //    _context.renderText( aChar, 1060, 310 );
    //    sprintf( aChar, "Factor 2: %d", pSpeciman->factor(2) );
    //    _context.renderText( aChar, 1060, 290 );
    //    sprintf( aChar, "Factor 3: %d", pSpeciman->factor(3) );
    //    _context.renderText( aChar, 1060, 270 );
    //    sprintf( aChar, "Factor 4: %d", pSpeciman->factor(4) );
    //} else {
    //    sprintf( aChar, "Specimen: " );
    //    _context.renderText( aChar, 1060, 370 );
    //    sprintf( aChar, "Factor 0: " );
    //    _context.renderText( aChar, 1060, 330 );
    //    sprintf( aChar, "Factor 1: " );
    //    _context.renderText( aChar, 1060, 310 );
    //    sprintf( aChar, "Factor 2: " );
    //    _context.renderText( aChar, 1060, 290 );
    //    sprintf( aChar, "Factor 3: " );
    //    _context.renderText( aChar, 1060, 270 );
    //    sprintf( aChar, "Factor 4: " );
    //}
#else
    char aChar[100];
    sprintf( aChar, "Damping    : %f", _fDamping );
    _context.renderText( aChar, 10, 50 );
    sprintf( aChar, "3D Control : %s", _b3D ? "True" : "False" );
    _context.renderText( aChar, 10, 30 );
    sprintf( aChar, "Obstacles  : %s", _bObstacle ? "True" : "False" );
    _context.renderText( aChar, 10, 10 );
#endif

    char aChar[1024];
    if ( _bBabble ) {
        sprintf( aChar, "Babbling : Reset(%d) Orient(%d)", _iBabSinceReset, _iBabOrient );
        _context.renderText( aChar, 550, 20 );
    } else if ( _bEval ) {
        sprintf( aChar, "Eval : " );
        _context.renderText( aChar, 550, 20 );
    } else {
        btQuaternion qC = _qA.inverse()*_qB;
        float     fAng = qC.getAngle();
        btVector3 vAxis = qC.getAxis();
        
        /*
        sprintf( aChar, "Play : Pos(%.2f,%.2f,%.2f|%.2f,%.2f,%.2f) Ang(%.2f,%.2f)",
                 _vDbgPos[0][0], _vDbgPos[0][1], _vDbgPos[0][2], 
                 _vDbgPos[1][0], _vDbgPos[1][1], _vDbgPos[1][2],
                 _fDbgAng[0], _fDbgAng[1] );
        _context.renderText( aChar, 550, 20 );
        sprintf( aChar, "Ange(%.2f) Axis(%.2f,%.2f,%.2f)",
                 fAng, vAxis[0], vAxis[1], vAxis[2] );
        _context.renderText( aChar, 550, 40 );
        */
    }

    //_renderPointer();

    SDL_GL_SwapBuffers( );

    return true;
}

bool
MyWindow::_handleEvents()
{
    SDL_Event   event;

	SDL_PollEvent(&event);
    //while ( SDL_PollEvent(&event) ) {
		//int iDegree=-1;
	    switch (event.type) {
            case SDL_KEYDOWN:
				{
                _bSplash = false;
                if (event.key.keysym.sym == SDLK_g) {
                } else if (event.key.keysym.sym == SDLK_r) {
                    _resetBulletScene(); 
                }else if (event.key.keysym.sym == SDLK_d){
					_executePush( push_right, 5 );
				}else if (event.key.keysym.sym == SDLK_a){
					_executePush( push_left, 5 );
				}else if (event.key.keysym.sym == SDLK_w){
					_executePush( push_up, 5 );
				}else if (event.key.keysym.sym == SDLK_s){
					_executePush( push_down, 5 );
				}else if (event.key.keysym.sym == SDLK_b){
                    _bBabble = !_bBabble;
                    _eBabPhase = Babble_Init;
				}else if (event.key.keysym.sym == SDLK_z){
                   _simulation.deserialize("Cylinder.txt");
				}else if (event.key.keysym.sym == SDLK_e){
                    _bEval = !_bEval;
                    _eEvalPhase = Eval_Init;
				}else if (event.key.keysym.sym == SDLK_c){//print to console
					btVector3 newTrans = _pActiveShape->getWorldTransform().getOrigin();
					std::cout<<"shape origin: "<<newTrans.x()<<", "<<newTrans.y()<<", "<<newTrans.z()<<std::endl;
		
					btVector3 com = _pTable->getWorldTransform().getOrigin();
					std::cout<<"table origin: "<<com.x()<<", "<<com.y()<<", "<<com.z()<<"\n\n\n";
				}else if (event.key.keysym.sym == SDLK_t){
			
				} else if (event.key.keysym.sym == SDLK_1) {
                    //_createShape(1); // Box
                } else if (event.key.keysym.sym == SDLK_2) { 
                    //_createShape(2); // Sphere
                } else if (event.key.keysym.sym == SDLK_3) { 
                    _createShape(3); // Cylinder
                } else if (event.key.keysym.sym == SDLK_4) {
                    _createShape(4); // Square
                } else if (event.key.keysym.sym == SDLK_5) { 
                    _createShape(5); // Triangle
                } else if (event.key.keysym.sym == SDLK_6) { 
                    _createShape(6); // Rectangle
                } else if (event.key.keysym.sym == SDLK_7) { 
                    //_createShape(7); // Stick
                } else if (event.key.keysym.sym == SDLK_8) { 
                    //_createShape(8); // Ooze
                }

			   static unsigned int s_iStickPos  = 36000;
               static unsigned int s_iOrientAng = 36000;
               switch( event.key.keysym.sym ){
                    case SDLK_LEFT:
						{
						_updateStickToPosition( (++s_iStickPos)%360 );
						}
                        break;
                    case SDLK_RIGHT:
						{
						_updateStickToPosition( (--s_iStickPos)%360 );
						}
                        break;
                    case SDLK_UP:
                        _resetShapePosition( btVector3(0.0f,0.0f,-12.5f), s_iOrientAng++%360 );
                        break;
                    case SDLK_DOWN:
                        {
                        _resetShapePosition( btVector3(0.0f,0.0f,-12.5f), s_iOrientAng--%360 );
                        }
                        break;
                    default:
                        break;
                }


                // The escape key will quit the game
                if ( event.key.keysym.sym != SDLK_ESCAPE ) {
                    break;  
                }

				
				}
            case SDL_MOUSEMOTION:
                _handleMouseMove( event.button.x, event.button.y );
                break;
            case SDL_MOUSEBUTTONDOWN:
                if ( event.button.button == SDL_BUTTON_LEFT ) {
                    _handleLeftMouseDown( event.button.x, event.button.y );
                }
                break;
            case SDL_MOUSEBUTTONUP:
                if ( event.button.button == SDL_BUTTON_LEFT ) {
                    _handleLeftMouseUp( event.button.x, event.button.y );
                }
                break;
            case SDL_QUIT:
			    _bQuit = 1;
			    break;
		    default:
			    break;
        //}
    }

    return true;
}

bool 
MyWindow::_handleLeftMouseDown( int x, int y )
{
    const btScalar fPickClampMouse = 30.f; // TOXIC: What is this?
    btVector3 rStart, rEnd;

    rEnd = _createPickRay(x,y);
    
    rStart = rEnd;
    rStart.setZ(-100.0f);

    btCollisionWorld::ClosestRayResultCallback pickCB(rStart,rEnd);
    _pBltDynWorld->rayTest(rStart,rEnd,pickCB);
    if (pickCB.hasHit()) {

        btRigidBody* pRigBdy = btRigidBody::upcast(pickCB.m_collisionObject);

        MyShape *pShp = NULL;
        try {
            pShp = dynamic_cast<MyShape*>(pRigBdy);
        } catch (...) {
        }

        if (!pShp || (pShp->type() < my_sphere) ) return true;

        pRigBdy->setActivationState(DISABLE_DEACTIVATION);
        
        pShp->setIsPicked(true);

        btVector3 vPos    = pickCB.m_hitPointWorld;
        btVector3 vLocPiv = pRigBdy->getCenterOfMassTransform().inverse() * vPos;

        // Create and add a constraint to the world
        btPoint2PointConstraint* pP2P = new btPoint2PointConstraint(*pRigBdy,vLocPiv);
        pP2P->m_setting.m_impulseClamp = fPickClampMouse;
        pP2P->m_setting.m_tau = 0.1f;
        _pBltDynWorld->addConstraint(pP2P);

        // Track the picking event
        _pPickConstr = pP2P;
        _pPickRigBdy = pRigBdy;
        _vPickREnd   = rEnd; 
        _vPickPos    = vPos;  
        _fPickDist   = (vPos-rStart).length();; 
    }

    return true;
}

bool
MyWindow::_handleMouseMove( int x, int y )
{
    // Handle any picked objects
    if (_pPickConstr) {

        // Only handle Point 2 Point constraints
        btPoint2PointConstraint* pP2P = static_cast<btPoint2PointConstraint*>(_pPickConstr);
        if (!pP2P)
            return true;

        btVector3 rStart, rEnd   = _createPickRay(x,y);
        btVector3 oPivot, iPivot = pP2P->getPivotInB(); 

        oPivot = iPivot;
        oPivot.setX(rEnd.getX());
        oPivot.setY(rEnd.getY());
        //oPivot.setZ(-22.0f);

        pP2P->setPivotB(oPivot);
    }

    {
        _pPickedShape = NULL;

        btVector3 rStart, rEnd;
        rEnd = _createPickRay(x,y);
        
        rStart = rEnd;
        rStart.setZ(-100.0f);

        btCollisionWorld::ClosestRayResultCallback pickCB(rStart,rEnd);
        _pBltDynWorld->rayTest(rStart,rEnd,pickCB);
        if (pickCB.hasHit()) {
            btRigidBody* pRigBdy = btRigidBody::upcast(pickCB.m_collisionObject);

            MyShape *pShp = NULL;
            try {
                pShp = dynamic_cast<MyShape*>(pRigBdy);
            } catch (...) {
            }

            if ( pShp && (pShp->type() >= my_sphere) ) {
                _pPickedShape = pShp;
            }
        }
    }

    return true;
}

bool
MyWindow::_handleLeftMouseUp( int x, int y )
{
    if (_pPickConstr) {

        MyShape *pShape = (MyShape*)_pPickRigBdy;
        pShape->setIsPicked(false);

        // Remove and delete the pick constraint
        _pBltDynWorld->removeConstraint(_pPickConstr);
        delete _pPickConstr;

        _pPickConstr = NULL;

        // Place the picked body back into a activation state
        _pPickRigBdy->forceActivationState(ACTIVE_TAG);
        _pPickRigBdy->setDeactivationTime(0.0f);
        _pPickRigBdy = NULL;
    }

    return true;
}

#if 0
bool 
MyWindow::_findNeighbors( btRigidBody *pRigBdy )
{
    btVector3 aVec[8] = { btVector3(-5, 0, 0),
                          btVector3( 5, 0, 0),
                          btVector3( 0,-5, 0),
                          btVector3( 0, 5, 0),
                          btVector3( 3.54, 3.54, 0),
                          btVector3(-3.54, 3.54, 0),
                          btVector3( 3.54,-3.54, 0),
                          btVector3( -3.54, -3.54, 0) };
    int         nHit = 0;
    btScalar	m[16];
    btDefaultMotionState *pMotState = (btDefaultMotionState*)pRigBdy->getMotionState();
    pMotState->m_graphicsWorldTrans.getOpenGLMatrix(m);
    
    btVector3 rS(m[12],m[13],m[14]), rE;

    glEnable(GL_POINT_SMOOTH);
    glEnable(GL_LINE_SMOOTH);

    glDisable(GL_LIGHTING);

    glColor4f(0.0f,0.0f,1.0f,1.0f);
    glPointSize( 5.0f );

    for ( int i=0 ; i<8 ; i++ ) {

        rE = rS - aVec[i];

#if 1
        glBegin(GL_POINTS);
        glVertex3f(rS[0],rS[1],rS[2]);
        glEnd();
#endif
       
        btCollisionWorld::ClosestRayResultCallback pickCB(rS,rE);
        _pBltDynWorld->rayTest(rS,rE,pickCB);
        if (pickCB.hasHit()) {
            btRigidBody* pRigBdy = btRigidBody::upcast(pickCB.m_collisionObject);

            nHit++;

            glColor4f(1.0f,1.0f,0.0f,1.0f);
        } else {
            glColor4f(1.0f,0.0f,0.0f,1.0f);
        }

#if 1
        glBegin(GL_POINTS);
        glVertex3f(rE[0],rE[1],rE[2]);
        glEnd();
#endif

    }

    glEnable(GL_LIGHTING);

    ((MyOozeBall*)pRigBdy)->setHappinessFactor(nHit/8.0f);

    /*
    glBegin(GL_LINE);
    glVertex3f(-10.0f,0.0f,-25.0f);
    glVertex3f(10.0f,0.0f,-25.0f);
    glEnd();
    */

    return true;
}
#endif

//////////////////////////////
// Code dump
/////

#if 0
    #if defined(USE_MIDI_FUNCTIONS)
    PlayNonMelodicPercussion(FIRST_GM_NON_MELOD_PERC+s_uiMidiInstruments[p->instrument(iMote)],127);
    #endif

    #ifdef USE_MIDI_FUNCTIONS
        StopAllNotesPlayed(127);
    #endif
#endif

bool 
MyWindow::_initConfig()
{
    return false;

    // Attemp to open a configuration file
    FILE *fp = fopen( "shafttu.config", "r" );
    if (!fp) 
        return false;

    int nBalls=0;
    fscanf( fp, "<MyOozeBall=%d>\n", &nBalls ); 

    if ( nBalls==0 )
        return false;

    int x, y, f[5], ang;

    for ( int n=0 ; n<nBalls ; n++ ) {
        fscanf( fp, "<Ball>x=%d,y=%d,f0=%d,f1=%d,f2=%d,f3=%d,f4=%d,ang=%d</Ball>\n", 
                &x, &y, &f[0], &f[1], &f[2], &f[3], &f[4], &ang );

        MyShape *pShape = (MyShape*)_createDynamicBltSphere( 2.0f, btVector3(float(x),float(y),-20.0f) );
        //MyOozeBall *pBall = (MyOozeBall*)_createDynamicOozeBall( 2.0f, btVector3(float(x),float(y),-20.0f) );        
        //pBall->setFactors(f);
        //pBall->setSearchAng(ang);
    }

    //fscanf( fp, "</MyOozeBall>\n" ); 


    fclose(fp);

    return true;
}

bool MyWindow::_resetShapePosition( const btVector3 &vOrgPos, float iAng )
{
#if 1
    float fAng = PI*iAng/180.0f;
    btQuaternion quat(btVector3(0,0,1),fAng);
    _pActiveShape->getWorldTransform().setOrigin(vOrgPos);
    _pActiveShape->getWorldTransform().setRotation(quat);
    _pActiveShape->forceActivationState(ACTIVE_TAG);
#else
    btTransform newTrans;
    _pActiveShape->getMotionState()->getWorldTransform(newTrans);
    newTrans.getOrigin() = vOrgPos;
    _pActiveShape->getMotionState()->setWorldTransform(newTrans);
#endif

	return true;
    
}

bool 
MyWindow::_updateStickToPosition( unsigned int iPos )
{
#if 1
	
	//ang=final position of stick in degrees
	btScalar ang   = btScalar(iPos + (180/PI*_pActiveShape->getWorldTransform().getRotation().getAngle()));
	//btVector3 stickOffset = btVector3(btScalar(cos(ang*PI/180.0f)), btScalar(sin(ang*PI/180.0f)), 0.0);
	
	btVector3 vMin, vMax;
	_pActiveShape->getAabb(vMin,vMax);
	btScalar fDiag = (vMax-vMin).length();
	btScalar	          m[16];
	btDefaultMotionState *pMotState = (btDefaultMotionState*)_pActiveShape->getMotionState();
    pMotState->m_graphicsWorldTrans.getOpenGLMatrix(m);
    btVector3 rS(m[12],m[13],m[14]), rE;
	rE = rS +  btVector3(btScalar(cos(ang*PI/180.0f)*fDiag), btScalar(sin(ang*PI/180.0f)*fDiag), 0.0f);

	//_rStart=a point outside of the shape
	//_rEnd=center of the shape
	_rStart = rE;
	_rEnd   = rS; 

	 btCollisionWorld::AllHitsRayResultCallback ray(_rStart,_rEnd);
    _pBltDynWorld->rayTest(_rStart,_rEnd,ray);

	bool bHit = ray.hasHit();
    if ( bHit ) {
		_vCol = btVector4(0.0f,1.0f,0.0f,1.0f);
		btAlignedObjectArray<btCollisionObject*> objArr = ray.m_collisionObjects;
		int i = 0;
		while(i < objArr.size()){
			btRigidBody          *pRigBdy = btRigidBody::upcast(objArr[i]);
			MyShape              *pShp    = dynamic_cast<MyShape*>(pRigBdy);
			if(pShp == _pActiveShape){
				btVector3 stickOffset(0.0f,0.0f,0.0f),
						  hitPoint = ray.m_hitPointWorld[i];
				const float eTol = 1e-3f;
				vMin += btVector3(eTol,eTol,eTol);
				vMax += btVector3(-eTol,-eTol,-eTol);

				// Offset the stick in the direction of the normal
				stickOffset = 0.6f *ray.m_hitNormalWorld[i];

				_rHit = hitPoint;
				_rNorm = hitPoint + 20*ray.m_hitNormalWorld[i];
 				_rCenter = ray.m_hitPointWorld[i] + stickOffset;
				_pStickShape->getWorldTransform().setOrigin(ray.m_hitPointWorld[i] + stickOffset);
				i = objArr.size();
				//_pStickShape->forceActivationState(ACTIVE_TAG);
				_pStickShape->setActivationState(ACTIVE_TAG);
				_pStickShape->setDeactivationTime(0.0f);

			}else{
				i++;
			}

		}
	} else {
		_vCol = btVector4(1.0f,0.0f,0.0f,1.0f);
	}
    
#else
	//ang=final position of stick in degrees
	btScalar ang   = btScalar(iPos + (180/PI*_pActiveShape->getWorldTransform().getRotation().getAngle()));
	btVector3 stickOffset = btVector3(btScalar(cos(ang*PI/180.0f)), btScalar(sin(ang*PI/180.0f)), 0.0);
	

	btVector3 vMin, vMax;
	_pActiveShape->getAabb(vMin,vMax);
	btScalar fDiag = (vMax-vMin).length();
	btScalar	          m[16];
	btDefaultMotionState *pMotState = (btDefaultMotionState*)_pActiveShape->getMotionState();
    pMotState->m_graphicsWorldTrans.getOpenGLMatrix(m);
    btVector3 rS(m[12],m[13],m[14]), rE;
	rE = rS +  btVector3(btScalar(cos(ang*PI/180.0f)*fDiag), btScalar(sin(ang*PI/180.0f)*fDiag), 0.0f);

	//_rStart=a point outside of the shape
	//_rEnd=center of the shape
	_rStart = rE;
	_rEnd   = rS;

	 //btCollisionWorld::ClosestRayResultCallback ray(_rStart,_rEnd);
	 btCollisionWorld::AllHitsRayResultCallback ray(_rStart,_rEnd);
    _pBltDynWorld->rayTest(_rStart,_rEnd,ray);

	bool bHit = ray.hasHit();
    if ( bHit ) {
		_vCol = btVector4(0.0f,1.0f,0.0f,1.0f);
		btAlignedObjectArray<btCollisionObject*> objArr = ray.m_collisionObjects;
		int i = 0;
		while(i < objArr.size()){
			btRigidBody          *pRigBdy = btRigidBody::upcast(objArr[i]);
			MyShape              *pShp    = dynamic_cast<MyShape*>(pRigBdy);
			if(pShp == _pActiveShape){
				_pStickShape->getWorldTransform().setOrigin(ray.m_hitPointWorld[i] + stickOffset);
				i = objArr.size();
				
				_pStickShape->forceActivationState(ACTIVE_TAG);
			}else{
				i++;
			}

		}
	} else {
		_vCol = btVector4(1.0f,0.0f,0.0f,1.0f);
	}
#endif				

    return true;
}

bool MyWindow::_executePush( MySimPushDir eDir, unsigned int iDis )
{    
	if ( _nStickMovIter == 0 ) {

        _vDbgPos[0] = _pActiveShape->getWorldTransform().getOrigin();
		_fDbgAng[0] = 180.0f/PI*_pActiveShape->getWorldTransform().getRotation().getAngle();
        _qA = _pActiveShape->getWorldTransform().getRotation();

		static float fMovDist = 5.0f;
		float x=0.0f, y=0.0f;
		_fPickDist = fMovDist;
		switch ( eDir ) 
        { 
		  case push_up : 
			  y=_fPickDist;
			  break;
		  case push_right : 
			  x=_fPickDist;
			  break;
		  case push_down : 
			  y=-_fPickDist;
			  break;
		  case push_left : 
			  x=-_fPickDist;
			  break;
		};

        _vStickPos = _pStickShape->getWorldTransform().getOrigin();
		_nStickMovIter = 100;/*int(iDis/_fPickDist);*///Dont know if this was meant to be an int...
		_fStickDis     = float(iDis);
        _vStickMov     = btVector3(x,y,0.0f);
		_pStickShape->setLinearVelocity(_vStickMov);

		_pStickShape->forceActivationState(ACTIVE_TAG);
        _pStickShape->setDeactivationTime(0.0f);
	}


	return true;
}

void MyWindow::_runBabble()
{
    const unsigned int iResetNum = 20;//50

    // Ensure everything is setup for babbling3
    if ( _eBabPhase == Babble_Init ) {

        _simulation.reset(_pActiveShape->type());
        
        // Ensure the boundary
        _pTable->getAabb(_bMin,_bMax);
	    
        // Reset the shape position
        //_resetShapePosition(btVector3(0.0f,0.0f,0.0f), ((_iBabOrient++)%360*PI)/180);
		_resetShapePosition(btVector3(0.0f,0.0f,-12.5f), _iBabOrient++);
        _iBabSinceReset = 0;

        // Update the phase to ready
        _eBabPhase = Babble_Ready;
    }


    // Execute a push if ready
    if ( _eBabPhase == Babble_Ready ) {

        if ( _invalid() || _iBabSinceReset > iResetNum ) {
			_resetShapePosition(btVector3(0.0f,0.0f,-12.5f), (_iBabOrient++));
            _iBabSinceReset = 0;
        }

        // Obtain the original position and angle
		_vBabOrigPos = _pActiveShape->getWorldTransform().getOrigin();
        _qStart      = _pActiveShape->getWorldTransform().getRotation();

        btQuaternion qTmp = _qOrig.inverse()*_qStart; 
        btVector3    vTmp = qTmp.getAxis();
        _fBabOrigAng = (180.0f/PI)*((vTmp[2] >= 0.0f) ? qTmp.getAngle() : (2*PI)-qTmp.getAngle());

        // Select an action
		 _chooseAction ( int(_fBabOrigAng), _objBabAct);

		// Update the stick position
        _updateStickToPosition(_objBabAct.iPos);

		//printf( "*** Pushing: iPos: %d, iDir: %d, iDis: %d\n", _objBabAct.iPos, _objBabAct.iDir, _objBabAct.iDis );

        // Execute a push based on the the chosen action
		_executePush((MySimPushDir) _objBabAct.iDir,_objBabAct.iDis);

		/*btVector3 centerOfMass = _pActiveShape->getCenterOfMassPosition(),
				  origin = _pActiveShape->getWorldTransform().getOrigin();
		std::cout<<"\n\nCenter of Mass: "<<centerOfMass.x()<<", "<<centerOfMass.y()<<", "<<centerOfMass.z()<<std::endl;
		std::cout<<"Origin: "<<origin.x()<<", "<<origin.y()<<", "<<origin.z()<<"\n\n";*/

        // Update the phase to pushing
        _eBabPhase = Babble_Pushing;
    }

    // Obtain the results when done
    if ( _eBabPhase == Babble_Pushing ) {

		if ( _nStickMovIter == 0 ) {
			btVector3 vLinVel = (_pActiveShape->getLinearVelocity()).absolute(),
					  vAngVel = (_pActiveShape->getAngularVelocity()).absolute(),
					  vZero(0.005f,0.005f,0.005f);

			bool bDone = vLinVel[0]<vZero[0] && 
						 vLinVel[1]<vZero[1] &&
						 vLinVel[2]<vZero[2] &&
						 vAngVel[0]<vZero[0] &&
						 vAngVel[1]<vZero[1] &&
						 vAngVel[2]<vZero[2];
			if ( bDone ) {
  
				// Obtain the new position and angle
				btVector3 fpos = _pActiveShape->getWorldTransform().getOrigin();
				_qEnd          = _pActiveShape->getWorldTransform().getRotation();

                btQuaternion qTmp = _qOrig.inverse()*_qEnd; 
                btVector3    vTmp = qTmp.getAxis();
                float finAngle = (180.0f/PI)*((vTmp[2] >= 0.0f) ? qTmp.getAngle() : (2*PI)-qTmp.getAngle());

				// Caculate the result
				_calculateResult(_fBabOrigAng,finAngle,_vBabOrigPos,fpos, _objBabRes);

				//printf( "*** Push Complete: %.2f %.2f %.2f\n", _objBabRes.fMotDis, _objBabRes.fMotAng, _objBabRes.fOriAng );
    
				// If still in bounds add to the simulation
                if ( !_invalid() ) {//or if _flipped

					if ( _objBabRes.isValid() ) {
						// Increment the reset counter
						_iBabSinceReset++;
					}

                    // Add 0 equivalent push and results
					_simulation.addResults(_objBabAct,_objBabRes);
					
                    // Add 1 equivalent push and results
                    _objBabAct.iDir    = (_objBabAct.iDir + 1) % 4;
                    _objBabAct.iOrient = (360+_objBabAct.iOrient-90)%360;
                    _objBabRes.fMotAng = (_objBabRes.fMotAng-90)<0 ? 360+_objBabRes.fMotAng-90 : _objBabRes.fMotAng-90;
                    _simulation.addResults(_objBabAct,_objBabRes);

                    // Add 2 equivalent push and results
                    _objBabAct.iDir    = (_objBabAct.iDir + 1) % 4;
                    _objBabAct.iOrient = (360+_objBabAct.iOrient-90)%360;
                    _objBabRes.fMotAng = (_objBabRes.fMotAng-90)<0 ? 360+_objBabRes.fMotAng-90 : _objBabRes.fMotAng-90;
                    _simulation.addResults(_objBabAct,_objBabRes);

                    // Add 3 equivalent push and results
                    _objBabAct.iDir    = (_objBabAct.iDir + 1) % 4;
                    _objBabAct.iOrient = (360+_objBabAct.iOrient-90)%360;
                    _objBabRes.fMotAng = (_objBabRes.fMotAng-90)<0 ? 360+_objBabRes.fMotAng-90 : _objBabRes.fMotAng-90;
                    _simulation.addResults(_objBabAct,_objBabRes);
                    
                    //printf("\n");
				}       

				// Update the phase ready
				_eBabPhase = Babble_Ready;
			}
		}
    }

    // Termination of the babble
    if ( _eBabPhase == Babble_Ready && _iBabOrient > 720 && _iBabSinceReset > iResetNum ) {//1440
        _eBabPhase = Babble_Done;
        _bBabble   = false;
	           
        _simulation.serialize(pFile[_pActiveShape->type()]);
		std::cout << "*** Babbling Terminated... " <<std::endl;

		_iBabOrient = 0;
    }
}

bool MyWindow::_invalid()
{
    btVector3 vMin, vMax;
	_pActiveShape->getAabb(vMin,vMax);

    btQuaternion qCur = _pActiveShape->getWorldTransform().getRotation();
    btQuaternion qTmp = _qOrig.inverse()*qCur; 
    btVector3    vTmp = qTmp.getAxis();

    // Flipped over
    if ( (vTmp[0] < -2e-2f || vTmp[0] > 2e-2f) &&
         (vTmp[1] < -2e-2f || vTmp[1] > 2e-2f) )
         return true;

    // If fallen off the table
	if ( (vMin[0] <= _bMin[0]) || 
         (vMax[0] >= _bMax[0]) || 
         (vMin[1] <= _bMin[1]) || 
         (vMax[1] >= _bMax[1]) )
		return true;

    return false;
}

void MyWindow::_calculateResult( float orgAngle, float finAngle, const btVector3 &opos, const btVector3 &fpos, obj_res &resA )
{ 
	resA.clear();

	// Early out if nothing changed
	if ( orgAngle == finAngle && opos == fpos ) {
		return;
	}

	// Calculate difference in Orientation
	resA.fOriAng = finAngle - orgAngle;
	if ( resA.fOriAng < 0.0f ) {
	   resA.fOriAng = 360.0f + resA.fOriAng;
	}
    if ( resA.fOriAng > 359.9f )
        resA.fOriAng = 0.0f;

	// Calculate linear distance of travel
    btVector3 vTrav = fpos-opos;
    resA.fMotDis = vTrav.length();
    if ( resA.fMotDis > 1e-3f ) {
        resA.fMotAng = 180.0f/PI*vTrav.angle(btVector3(1.0f,0.0f,0.0f));
        if ( vTrav[1] < 0.0f ) resA.fMotAng = 360.0f - resA.fMotAng;

        if ( resA.fMotAng > 359.9f )
            resA.fMotAng = 0.0f;
    }
}

void MyWindow::_chooseAction(int orientAngle, obj_act  &objA)
{ 
   
   objA.iOrient =  orientAngle;
   objA.iPos    =  rand() % 360;
   objA.iDir    =  rand() % push_num;
   objA.iDis    =  3;
      
}


void MyWindow::_runEval()
{
	if(_dtotalMoves > _maxPushes){
		_atMaxPushes = true;
	}

	// Termination of the Eval
    if ( (_eEvalPhase == Eval_Ready && (_bShapeAtMarker||_invalid())) || _atMaxPushes) 
	{
		std::cout<<_pActiveShape->type()<<" ";
		std::cout<<_iConfigNum<<" ";

		if(_invalid())
		{
			std::cout<<"ERROR "; //printf("\nERROR - Object out of bounds! Test case failed!\n");
		}
		else if (_bShapeAtMarker)
		{
			std::cout<<"SUCCESS "; //printf("\nSUCCESS!\n");
			
		}else{
			std::cout<<"FAILURE "; //printf("\nFAILURE!\n");
		}
			
#if 1
        printf( " Orient: %.2f|%.2f Position: %.2f@%.2f\n", 
                     _fMarkerAngle, _objEvalRes.fOriAng, _objEvalRes.fMotDis, _objEvalRes.fMotAng );
#else
		std::cout<<_dtotalMoves<<" ";
		std::cout<<_objEvalRes.fMotDis<<" "<<_objEvalRes.fMotAng<<" "<<_objEvalRes.fOriAng<<std::endl;
#endif

		//if(_dtotalMoves != 0){
		//	printf("\n%d pushes to termination\n", _dtotalMoves);
		//	printf("...fMotAng: %.3f\n",     _objEvalRes.fMotAng);//_objEvalRes
		//	printf("...fMotDis: %.3f\n",     _objEvalRes.fMotDis);
		//	printf("...fOriAng: %.3f\n\n\n", _objEvalRes.fOriAng);
		//}

		//one eval pass done - i.e one record run from file
		//delete the first record and continue
		_simulation._vConfig.erase(_simulation._vConfig.begin());
		_bShapeAtMarker = false;
		_dtotalMoves = 0;
		_atMaxPushes = false;

			if(_simulation._vConfig.size() != 0){
				_setupEval();
			}

		//if config vector is completely empty, then eval phase is over
		if  ((_simulation._vConfig.empty()))
		{
			_eEvalPhase = Eval_Done;
			_bEval   = false;
			_iConfigNum = 0;
				
	        //_simulation.serialize(".txt"); // TOXIC: Needs to mangle based off of shape/test
		    //std::cout << "*** Evaluation Terminated... " <<std::endl;
		}

		
	}

	// Ensure everything is setup for evaluation
    if (( _eEvalPhase == Eval_Init)&& !(_bShapeAtMarker)) {
	    //Get the setup values from the config file shaftu
	    if(_simulation.readFile()) {
	      _iEvalPasses = _simulation._vConfig.size(); 
	      _eEvalPhase = Eval_Setup;
	    }	

		// Ensure the boundary
        _pTable->getAabb(_bMin,_bMax);
	}
	  	
	if((_eEvalPhase == Eval_Ready) && !(_bShapeAtMarker))
	{
	    
		//get current details about the shape
		_vShapeOrigPos = _pActiveShape->getWorldTransform().getOrigin();
        _qStart         = _pActiveShape->getWorldTransform().getRotation();

        btQuaternion qTmp = _qOrig.inverse()*_qStart; 
        btVector3    vTmp = qTmp.getAxis();
        _fShapeOrigAng = (180.0f/PI)*((vTmp[2] >= 0.0f) ? qTmp.getAngle() : (2*PI)-qTmp.getAngle());

		// calculate distance between origin and destination (marker).
		_calculateResult(_fShapeOrigAng, _fMarkerAngle, _vShapeOrigPos, _vMarkerPosition, _objEvalRes);

		//check to see if orientation and distance are within a certain tolerance, if so end evaluation
		float tolerance = 3.6f;
		if((_objEvalRes.fMotDis<0.50f && _objEvalRes.fMotDis>-0.50f) && (_objEvalRes.fOriAng<tolerance && _objEvalRes.fOriAng>-tolerance))//_fitsOrientation(_objEvalRes.fOriAng)
		{
		     _bShapeAtMarker = true;
		    //stop evaluation
		
		} else {

		    //below code returns objres - use it to get required  action
		    obj_res res = _simulation.obtainClosetResult( _fShapeOrigAng, _objEvalRes.fOriAng,_objEvalRes.fMotDis,_objEvalRes.fMotAng);
			_objEvalAct.iOrient = res.iterAct->iOrient;
			_objEvalAct.iPos    = res.iterAct->iPos;
			_objEvalAct.iDir    = res.iterAct->iDir;
			_objEvalAct.iDis    = res.iterAct->iDis;
	    
            {
                MyPoint pntA(_vShapeOrigPos[0],_vShapeOrigPos[1],-12.5f);
            
                float r = PI/180.0f * res.fMotAng;
                float x = _vShapeOrigPos[0] + res.fMotDis*cos(r); 
                float y = _vShapeOrigPos[1] + res.fMotDis*sin(r); 
                MyPoint pntB(x,y,-12.5f);

                _vEvalPred.push_back( pntA );
                _vEvalPred.push_back( pntB );
            }

		    // Update the stick position
            _updateStickToPosition(_objEvalAct.iPos);

		    //apply action to object - execute push
		    _executePush((MySimPushDir) _objEvalAct.iDir, _objEvalAct.iDis);

		    //Increment total number of moves
		    _dtotalMoves++;
			//printf("Move number: %d\n", _dtotalMoves);

		    // Update the phase to pushing
		    _eEvalPhase = Eval_Pushing; 
		}
	
	}

	    // Obtain the results when done
    if ( _eEvalPhase == Eval_Pushing && !(_bShapeAtMarker)) {

		if ( _nStickMovIter == 0 ) {
			btVector3 vLinVel = (_pActiveShape->getLinearVelocity()).absolute(),
					  vAngVel = (_pActiveShape->getAngularVelocity()).absolute(),
					  vZero(0.005f,0.005f,0.005f);

			bool bDone = vLinVel[0]<vZero[0] && 
						 vLinVel[1]<vZero[1] &&
						 vLinVel[2]<vZero[2] &&
						 vAngVel[0]<vZero[0] &&
						 vAngVel[1]<vZero[1] &&
						 vAngVel[2]<vZero[2];
			if ( bDone ) {
  
				// Obtain the new position and angle
				btVector3 fpos = _pActiveShape->getWorldTransform().getOrigin();
                _qEnd          = _pActiveShape->getWorldTransform().getRotation();

                btQuaternion qTmp = _qOrig.inverse()*_qEnd; 
                btVector3    vTmp = qTmp.getAxis();
                float finAngle = (180.0f/PI)*((vTmp[2] >= 0.0f) ? qTmp.getAngle() : (2*PI)-qTmp.getAngle());

				// Caculate the result
				_calculateResult( _fShapeOrigAng, finAngle, _vShapeOrigPos, fpos, _objEvalRes);

                {
                    MyPoint pntA(_vShapeOrigPos[0],_vShapeOrigPos[1],-12.5f);
            
                    float r = PI/180.0f * _objEvalRes.fMotAng;
                    float x = _vShapeOrigPos[0] + _objEvalRes.fMotDis*cos(r); 
                    float y = _vShapeOrigPos[1] + _objEvalRes.fMotDis*sin(r); 
                    MyPoint pntB(x,y,-12.5f);

                    _vEvalAct.push_back( pntA );
                    _vEvalAct.push_back( pntB );
                }

				// If still in bounds add to the simulation
				if ( !_invalid() ) {

					/*if ( _objEvalRes.isValid() ) {
												
					}*/

					_simulation.addResults(_objEvalAct,_objEvalRes);
					//printf("\n");
				}       

				// Update the phase ready
				_eEvalPhase = Eval_Ready;
			}
		}
    }

    

	if (( _eEvalPhase == Eval_Setup) && !(_bShapeAtMarker ) && !(_simulation._vConfig.empty())) {
		_setupEval();
	 }
}
void MyWindow::_setupEval(){
		_iConfigNum++;
		//printf("\n****************************************************");
		//printf("\nSeting up evaluation parameters...\n");
		//printf("\n

		    // Stop the stick
		    _nStickMovIter = 0;
            _fStickDis     = 0.f;
            _vStickMov     = btVector3(0.0f,0.0f,0.0f);
            _pStickShape->setLinearVelocity(_vStickMov);

		// Obtain the current active shape
        unsigned int eShp = _simulation._vConfig._Myfirst->shape; 

        // Load the necessary
        _simulation.reset( eShp );
        _simulation.deserialize(pFile[eShp]);

        _vEvalAct.clear();
        _vEvalPred.clear();

        _createShape(eShp);

		 btVector3 shapePosition(_simulation._vConfig._Myfirst->aInitShapePos[0],_simulation._vConfig._Myfirst->aInitShapePos[1], _simulation._vConfig._Myfirst->aInitShapePos[2]);
	    //Position the shape
	     _resetShapePosition(shapePosition,  _simulation._vConfig._Myfirst->initShapeAng);
		    
		//Position the shape marker
		 btVector3 temp(_simulation._vConfig._Myfirst->aInitMarkerPos[0],  _simulation._vConfig._Myfirst->aInitMarkerPos[1],  _simulation._vConfig._Myfirst->aInitMarkerPos[2]);
		_vMarkerPosition = temp;
		_fMarkerAngle = _simulation._vConfig._Myfirst->initMarkerAng;

		// Set the marker the position
		_pMarker->setPosition(_vMarkerPosition);
		_pMarker->setOrientDeg(_fMarkerAngle);

		_eEvalPhase = Eval_Ready;
}
bool MyWindow::_fitsOrientation(float angDif){
	MyShapeType sType = _pActiveShape->type();
	float tolerance = 3.6,
		  tempO[5] = {90.0f, 120.0f, 180.0f, 240.0f, 270.0f};

	switch(sType){
	case my_cylinder:
		return true;
	case my_square:
		if(angDif<tolerance && angDif>-tolerance || 
			angDif<tempO[0]+tolerance && tempO[0]-tolerance || 
			angDif<tempO[2]+tolerance && tempO[2]-tolerance || 
			angDif<tempO[4]+tolerance && tempO[4]+tolerance)
		{
			return true;
		}else{
			return false;
		}
	case my_triangle:
		if(angDif<tolerance && angDif>-tolerance|| 
			angDif<tempO[1]+tolerance && tempO[1]-tolerance || 
			angDif<tempO[3]+tolerance && tempO[3]-tolerance)
		{
			return true;
		}else{
			return false;
		}
	case my_retangle:
		if(angDif<tolerance && angDif>-tolerance || angDif<tempO[2]+tolerance && tempO[2]-tolerance){
			return true;
		}else{
			return false;
		}
	};
	return false;
}
bool MyWindow::_createShape( unsigned int eShp )
{

    if ( _pActiveShape ) {
        // Delete the associated motion state
        btMotionState *pMotState = _pActiveShape->getMotionState();
        if (pMotState) delete pMotState;
    
        // Remove the object from the world and delete it
        _pBltDynWorld->removeCollisionObject(_pActiveShape);
        if (_pActiveShape) delete _pActiveShape;
        _pActiveShape = NULL;
    }

    // Create new shape
	switch( eShp ) {
		case 3:
		    _pActiveShape = (MyShape*) _createDynamicBltCylinder( btVector3(6.0f,7.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f), 10.0f );
		    break;
		case 4:
		    _pActiveShape = (MyShape*) _createDynamicBltSquare( btVector3(6.0f,6.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f), 10.0f);
		    break;
		case 5:
		    _pActiveShape = (MyShape*) _createDynamicBltTriangle(12.0f, 2.0f, btVector3(0.0f, 0.0f,-20.0f), 10.0f);
		    break;
        case 6:
		    _pActiveShape = (MyShape*) _createDynamicBltRectangle( btVector3(12.0f,6.0f,2.0f), btVector3(0.0f, 0.0f,-20.0f), 10.0f);
		    break;
	};	

    if ( _pActiveShape ) {
        btDefaultMotionState* pMotState = (btDefaultMotionState*)_pActiveShape->getMotionState();
        pMotState->m_graphicsWorldTrans = pMotState->m_startWorldTrans;
        _pActiveShape->setCenterOfMassTransform( pMotState->m_graphicsWorldTrans );
        _pActiveShape->setInterpolationWorldTransform( pMotState->m_startWorldTrans );
        _pActiveShape->forceActivationState(ACTIVE_TAG);
        _pActiveShape->setDeactivationTime(0);

        _pActiveShape->setLinearVelocity(btVector3(0,0,0));
        _pActiveShape->setAngularVelocity(btVector3(0,0,0));

        _qOrig = _pActiveShape->getWorldTransform().getRotation();
    }

	// Set the active shape as the marker shape
	if (_pMarker) _pMarker->setShape(_pActiveShape);
	_pMarker->setOrientDeg(40.0f);

    return true;
}

